#include "dynamic_cloth_matching.h"
#include <dynamic_reconfigure/server.h>
#include <ros/node_handle.h>
#include <stdio.h>
#include <std_msgs/String.h>
#include <roseus/StringStamped.h>
#include "dynamic_cloth_matching/dynamic_cloth_matching_paramsConfig.h"
#define MODE 1 //0:save mode 1:comparemode

#define JISSEN
// #define RESEARCH


using namespace cv;
using namespace std;
namespace enc = sensor_msgs::image_encodings;

class dynamicClothMatchingNodeImpl:
  public dynamicClothMatchingNode
{
  ros::NodeHandle nh_;

  image_transport::ImageTransport it_;
  ros::Subscriber caminfo_sub_;

  ros::Publisher pubState_;

  image_transport::Subscriber image_sub_;
  image_transport::Publisher image_pub_;
  image_geometry::PinholeCameraModel cam_model_;

  dynamic_reconfigure::Server<dynamic_cloth_matching::dynamic_cloth_matching_paramsConfig> server;
  dynamic_reconfigure::Server<dynamic_cloth_matching::dynamic_cloth_matching_paramsConfig>::CallbackType f;


  double sprh_param, hist_param, part_param;

  cv::Mat src_img;
  FB fb;
  vector<Point2f> flowPoint, flowScale;
  vector<Point2f> extflowPoint, extflowScale;
  bool initFlag;

  compareCloth cc;
  double thre_x,thre_y,thre_theta;

  const char *data_c;
  const char *step_c;

  int jissen_count;

#ifdef RESEARCH
  compareCloth cs;
#endif


public:
  dynamicClothMatchingNodeImpl()
    : it_(nh_)
  {
    ros::NodeHandle private_node_handle_("~");
    private_node_handle_.param("sprh_param", sprh_param, double(1.0));
    private_node_handle_.param("hist_param", hist_param, double(1.0));
    private_node_handle_.param("part_param", part_param, double(1.0));


    f = boost::bind(&dynamicClothMatchingNodeImpl::dynamic_param_cb, this, _1, _2);
    server.setCallback(f);


    caminfo_sub_ = nh_.subscribe("camera_info", 1, &dynamicClothMatchingNodeImpl::caminfoCb, this);

    image_sub_ = it_.subscribe("image", 1, &dynamicClothMatchingNodeImpl::imageCb, this);


    pubState_ = nh_.advertise<roseus::StringStamped>("state_result",1);

    initFlag = true;

    rospack::ROSPack rp;
    string dataFileName;
    try{
      rospack::Package *p = rp.get_pkg("dynamic_cloth_matching");
      if(p!=NULL) dataFileName = p->path;
    } catch (std::runtime_error &e)
      {
      }

    std::string data = dataFileName + std::string("/data");
    std::string step = dataFileName + std::string("/step");
    data_c = data.c_str();
    step_c = step.c_str();
    cout << data_c << endl;
    cout << step_c << endl;

#ifdef RESEARCH
    //to use reserch
    cc.init(data_c, 5, 11);
    cs.init(step_c, 1, 5);
#endif

#ifdef JISSEN
    //to use jissen
    // cc.init(data_c, 1, 4);
    cc.init(data_c, 1, 1);
#endif

    thre_x = 0;
    thre_y = 0;
    thre_theta = 0;

    jissen_count = 0;

  }
  ~dynamicClothMatchingNodeImpl()
  {
  }

  virtual void caminfoCb(const sensor_msgs::CameraInfoConstPtr& msg)
  {
    cam_model_.fromCameraInfo(msg);
  }

  void dynamic_param_cb(dynamic_cloth_matching::dynamic_cloth_matching_paramsConfig &config, uint32_t level) {
    ROS_INFO("\nset sprh_param: %f\nset hist_param: %f\nset part_param: %f\nthre_x: %f\nthre_y: %f\nthre_theta: %f\n", config.sprh_param,config.hist_param,config.part_param, config.thre_x, config.thre_y, config.thre_theta);
    vector<float> Thre, outthre;
    Thre.push_back(config.sprh_param);
    Thre.push_back(config.hist_param);
    Thre.push_back(config.part_param);
    cc.setThreVec(Thre);
    thre_x = config.thre_x;
    thre_y = config.thre_y;
    thre_theta = config.thre_theta;
    // cc.getThreVec(&outthre);
    // for( uint i = 0; i <outthre.size() ;i++)
    //   cout << "outthre " << outthre[i] << endl;
  }

  virtual void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {

    cv_bridge::CvImagePtr cv_ptr;
    try
      {
	cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
      }
    catch(cv_bridge::Exception& e)
      {
	ROS_ERROR("cv_bridge exception: %s", e.what());
	return;
      }
    src_img = cv_ptr->image;
    resizeSrcImg(src_img, src_img, 0.7, INTER_LINEAR);

    if(initFlag)
      {
	initProc();
      }
    fb.calcFB(src_img);
    fb.getFlowPoint(&flowPoint);
    fb.getFlowScale(&flowScale);

    extractRangeFlow(flowPoint, flowScale, &extflowPoint, &extflowScale, 100, 20);
    vector< pair<P2d,P2d> > valss;
    convertflowPoint2P2d(extflowPoint, extflowScale, &valss);
    for(uint i = 0; i < valss.size() ; i++)
      {
        valss[i].second.second += M_PI*0.5;
        if(valss[i].second.second > M_PI)
          {
            valss[i].second.second -= M_PI;
            valss[i].second.second += -M_PI;
          }
      }
    pair<P2d,P2d> mean, var;
    calcMeanVar(valss,&mean,&var);

    // cout << "mean " << mean.first.first <<  " mean " << mean.first.second <<
    //   " mean " << mean.second.first << " mean " << mean.second.second << endl;

    // cout << "var " << var.first.first <<  " var " << var.first.second <<
    //   " var " << var.second.first << " var " << var.second.second << endl;

    vector< pair<P2d,P2d> > vals;
    convertflowPoint2P2d(extflowPoint, extflowScale, &vals);
    cv::Mat flow_image;
    display(16, src_img, flow_image, vals, "flow_image");
    imshow("src_img", src_img);

    int image_count = 0;

    char c = waitKey(10);
    cv::Mat output_img;
    if(MODE == 1)
      {
	cc.stateVecMatch(image_count, extflowPoint, extflowScale, src_img, output_img);
#ifdef RESEARCH
	cs.stateVecMatch(image_count, extflowPoint, extflowScale, src_img, output_img);
#endif
      }
    if(c == 'w')
      {
	waitKey(0);
      }

    
    cout << "stateNum " << cc.getTempState() << endl;
    int stateNumber = cc.getTempState();
    std::string str;
    //    std_msgs::String s;
    roseus::StringStamped s;
    double std_div_x, std_div_y, std_div_theta;
    double mean_x, mean_y, mean_theta;
    std_div_x = sqrt(var.first.first);
    std_div_y = sqrt(var.first.second);
    std_div_theta = sqrt(var.second.second);
    mean_x = mean.first.first;
    mean_y = mean.first.second;
    mean_theta = mean.second.second;

    if(stateNumber == 1)
      {
      cout << "var.x " << std_div_x << endl;
      cout << "var.y " << std_div_y << endl;
      cout << "var.angle " << std_div_theta << endl;
      }
    if(stateNumber == 1)
      {
      cout << "mean.x " << mean_x << endl;
      cout << "mean.y " << mean_y << endl;
      cout << "mean.angle " << mean_theta << endl;
      }
    cv::Mat jissen_out(384,512,CV_32FC1);
    if(std_div_x < thre_x && std_div_y < thre_y && std_div_theta < thre_theta){
      if(mean_x > 100 && mean_x < 200 && mean_y > 100 ){
        if ( stateNumber == 1){
          str = string("both");
	  cc.getMatchImg(jissen_out);
        }else{
          str = string("nan");

          // cv::Mat blackImage = Mat::zeros(src_img.rows, src_img.cols, CV_32F);
          // char *result_dir;
          // sprintf(result_dir, "matchResult:%s" ,data_c);
          // cout << "hgoehgoehgoe" << result_dir << endl;
          // imshow(result_dir , blackImage);

        }
        // switch (stateNumber){
        // case 1 :
        //   str = string("both");
        //   // case 2 :
        //   //   str = string("right");
        //   // case 3 :
        //   //   str = string("left");
        //   // case 4 :
        //   //   str = string("nan");
        // case -1 :
        //   str = string("nan");
        // }

        s.header.frame_id="hoge";
        s.header.stamp = ros::Time::now();
        s.data=str;
        cout << "current hand state          " << str << endl;
        if(pubState_.getNumSubscribers() > 0)
          {
            pubState_.publish(s);

          }
      }
    }
    // imshow("jissen_out", jissen_out);
    // vector<cv::Mat> imageVec;
    // vector<Point> pointVec;
    // resizeSrcImg(src_img, src_img, 0.57, INTER_LINEAR);
    // resizeSrcImg(flow_image, flow_image, 0.57,INTER_LINEAR);
    // imageVec.push_back(jissen_out);
    // imageVec.push_back(src_img);
    // imageVec.push_back(flow_image);
    // pointVec.push_back(Point(0,0));
    // pointVec.push_back(Point(512,0));
    // pointVec.push_back(Point(512,192));
    // cv::Mat combine_img;
    // combineImage(imageVec, pointVec, combine_img);
    // imshow("comb" , combine_img);
    // char jissen_out_name[128];
    // sprintf(jissen_out_name, "/home/jsk/oya/Pictures/project-dsc/prezen_movie/black/image%04d.ppm" ,jissen_count);
    // jissen_count++;
    // imwrite(jissen_out_name, combine_img);



  }

  virtual void initProc()
  {
    fb.firstProc(src_img);

#ifdef RESEARCH
    vector<float> thre;
    thre.push_back(config.sprh_param);
    thre.push_back(config.hist_param);
    thre.push_back(config.part_param);
    cs.setThreVec(thre);

    vector<int> stateVec;
    stateVec.push_back(1);
    stateVec.push_back(2);
    stateVec.push_back(3);
    stateVec.push_back(4);
    stateVec.push_back(6);
    stateVec.push_back(7);
    stateVec.push_back(10);
    stateVec.push_back(11);
    stateVec.push_back(12);
    stateVec.push_back(13);
    stateVec.push_back(14);
    cc.sm.setAliasVector(stateVec);
    // cc.sm.showAliasVector();
    cc.sm.setProbability(1, 2, 1.0);
    cc.sm.setProbability(2, 3, 1.0);
    cc.sm.setProbability(3, 4, 1.0);
    cc.sm.setProbability(1, 6, 1.0);
    cc.sm.setProbability(1, 7, 1.0);
    cc.sm.setProbability(1, 10, 1.0);
    cc.sm.setProbability(1, 11, 1.0);
    cc.sm.setProbability(1, 12, 1.0);
    cc.sm.setProbability(1, 13, 0.0);
    cc.sm.setProbability(1, 14, 0.0);
    cc.sm.showProbabilityMap();

    vector<int> stepVec;
    stepVec.push_back(1);
    stepVec.push_back(2);
    stepVec.push_back(3);
    stepVec.push_back(4);
    stepVec.push_back(5);
    cs.sm.setAliasVector(stepVec);
    cs.sm.setProbability(1, 2, 1.0);
    cs.sm.setProbability(2, 3, 0.2);
    cs.sm.setProbability(3, 4, 0.6);
    cs.sm.setProbability(4, 5, 1.0);
    cs.sm.showProbabilityMap();
    cc.setState(1);
    cs.setState(1);
#endif

#ifdef JISSEN
  vector<float> ccThre, csThre;
  //set cc threshold
  ccThre.push_back(1.0);
  ccThre.push_back(1.0);
  ccThre.push_back(1.0);
  cc.setThreVec(ccThre);
  thre_x = 75;
  thre_y = 85;
  thre_theta = 2.0;

  vector<int> stateVec;
  stateVec.push_back(1);
  // stateVec.push_back(2);
  // stateVec.push_back(3);
  // stateVec.push_back(4);
  cc.sm.setAliasVector(stateVec);
  // cc.sm.setProbabilityEach(1, 2, 1.0);
  // cc.sm.setProbabilityEach(2, 3, 1.0);
  // cc.sm.setProbabilityEach(1, 3, 1.0);
  // cc.sm.setProbabilityEach(1, 4, 1.0);
  // cc.sm.setProbabilityEach(2, 4, 1.0);
  // cc.sm.setProbabilityEach(3, 4, 1.0);
  cc.sm.showProbabilityMap();
  cc.setState(1);
#endif



    initFlag = false;

  }


};



dynamicClothMatchingNodePtr CreateDynamicClothMatchingNode()
{
  return dynamicClothMatchingNodePtr(new dynamicClothMatchingNodeImpl());
}

